#include "ros_modbus.h"
#include"CRC16.h"
#include"config.h"
using namespace std;
using namespace boost::asio;
//串口相关对象
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev, "/dev/ttyUSB0");
boost::system::error_code err;
/********************************************************
            串口发送接收相关常量、变量、共用体对象
********************************************************/
MODBUS modbus;
const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};
extern unsigned char  write_CHASSIS0[8];
extern unsigned char  write_CHASSIS1[8];

u8 RS485_RX_BUFF[2048];//接收缓冲区2048字节 RS485_RX_buff[264];
u16 RS485_RX_CNT=0;//接收计数器
u8 RS485_FrameFlag=0;//帧结束标记
u8 RS485_TX_BUFF[2048];//发送缓冲区 RS485_TX_buff[300];
u16 RS485_TX_CNT=0;//发送计数器

//发送左右轮速控制速度共用体
union sendData
{
	short d;
	unsigned char data[2];
}leftVelSet,rightVelSet;

//接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
union receiveData
{
	short d;
	unsigned char data[2];
}leftVelNow,rightVelNow,angleNow;

/**
  * @brief          串口参数初始化
  * @param[in]      none
  * @retval         none
  */
void serialInit()
{
    sp.set_option(serial_port::baud_rate(115200));
    sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
    sp.set_option(serial_port::parity(serial_port::parity::none));
    sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
    sp.set_option(serial_port::character_size(8));    
}

/**
  * @brief         将Modbus消息发送至下位机
  * @param[in]     *buf:需要发送数据的指针
  * @retval         none
  */
 void writeModbus_in(unsigned char addr,unsigned int reg,unsigned int value)
 {
  //   unsigned char buf[] ;
    unsigned char cont = 0;
    unsigned int crc;
     modbus.Sendbuf[cont++] = addr;
     modbus.Sendbuf[cont++] = 0x06; //写
     modbus.Sendbuf[cont++] = reg /256;     //寄存器高8位
     modbus.Sendbuf[cont++] = reg%256;
    modbus.Sendbuf[cont++] = value /256;
    modbus.Sendbuf[cont++] = value%256;
    crc = crc16(modbus.Sendbuf,cont);
    modbus.Sendbuf[cont++] = crc/256;  
    modbus.Sendbuf[cont++] = crc%256;
    boost::asio::write(sp, boost::asio::buffer(modbus.Sendbuf));
    ROS_INFO("transfer_ok!");
 }
 /**
  * @brief         返回发送状态
  * @param[in]     none
  * @retval        none
  */
bool writeModbus()
{    
    // 通过串口下发数据
    //unsigned char bufs[] = write_CHASSIS0;
    boost::asio::write(sp, boost::asio::buffer(write_CHASSIS0));
    ROS_INFO("transmit Ok");
    return true;
}


/**
  * @brief         从下位机读取数据
  * @param[in]     none
  * @retval        none
  *
  */
bool read_modbus(void)
{
    char i, length = 0;
    unsigned char checkSum;
    unsigned char buf[150]={0};
    //=========================================================
    //此段代码可以读数据的结尾，进而来进行读取数据的头部
    try
    {
        boost::asio::streambuf response;
        boost::asio::read_until(sp, response, "\r\n",err);   
        copy(istream_iterator<unsigned char>(istream(&response)>>noskipws),
        istream_iterator<unsigned char>(),
        buf); 
    }  
    catch(boost::system::system_error &err)
    {
        ROS_INFO("read_until error");
    } 
    //=========================================================        

    // 检查信息头
    unsigned int  crc;
    unsigned char crch, crcl;

    if ((buf[0]!=0x01)&&(buf[0]!=0x02)&&(buf[0]!=0x08)) //不是从机地址不处理
    {                   
        return;         
    } 
    //地址相符时，再对本帧数据进行校验
    crc = GetCRC16(buf, len-2);  //计算CRC校验值
    crch = crc >> 8;
    crcl = crc & 0xFF;
    if ((buf[len-2]!=crch) || (buf[len-1]!=crcl))
    {
        return;   //如CRC校验不符时直接退出
    }
    //地址和校验字均相符后，解析功能码，执行相关操作
    switch (buf[1])//shujubuf[4];//数据缓存 0存温度,1存湿度,2存LED,3存REL
    {
        case 0x03:  //对从机的03回复数据进行处理
            		if (buf[2]==0x02) //数据2字节
						{  
         				if(buf[0]==0x02)//温度从机地址
							{
								shujubuf[0]=buf[4];//数据缓存 0存温度
							}
							else if(buf[0]==0x08)//湿度从机地址
							{
								shujubuf[1]=buf[4];//数据缓存1存湿度
							}
						}
						else
                  {
							return;//指令不对退出
						}
            
        case 0x06:  //对从机的06回复数据进行处理//shujubuf[4];//数据缓存
            		if ((buf[2]==0x20) && (buf[3]==0x00))//数据地址
					{  
 						shujubuf[2]=buf[5];//把的状态存入数组
					}

					else if((buf[2]==0x20) && (buf[3]==0x01))//REL数据地址
					{
						shujubuf[3]=buf[5];//把REL的状态存入数组
					}

        case 0x10:  //对从机的10回复数据进行处理//shujubuf[4];//数据缓存
            		if ((buf[2]==0x20) && (buf[3]==0x00))//数据地址
					{  
 						shujubuf[2]=buf[5];//把的状态存入数组
					}

					else if((buf[2]==0x20) && (buf[3]==0x01))//REL数据地址
					{
						shujubuf[3]=buf[5];//把REL的状态存入数组
					}
	
            
        	 default:  //其它不支持的功能码
            			buf[1] |= 0x80;  //功能码最高位置1
            			buf[2] = 0x01;   //设置异常码为01-无效功能
            			len = 3;
            			break;
    }
    return true;
}

u16 startRegAddr;
u16 RegNum;
u16 calCRC;
/**
  * @brief         从下位机读取数据
  * @param[in]     none
  * @retval        none
  *
  */
void RS485_Service(void)
{
        u16 recCRC;
        if(RS485_FrameFlag==1)
        {
                if(RS485_RX_BUFF[0]==RS485_Addr)//地址正确
                {
                        if((RS485_RX_BUFF[1]==01)||(RS485_RX_BUFF[1]==02)||(RS485_RX_BUFF[1]==03)||(RS485_RX_BUFF[1]==05)||(RS485_RX_BUFF[1]==06)||(RS485_RX_BUFF[1]==15)||(RS485_RX_BUFF[1]==16))//功能码正确
                  {
                                startRegAddr=(((u16)RS485_RX_BUFF[2])<<8)|RS485_RX_BUFF[3];//获取寄存器起始地址
                                if(startRegAddr<1000)//寄存器地址在范围内
                                {
                                        calCRC=CRC_Compute(RS485_RX_BUFF,RS485_RX_CNT-2);//计算所接收数据的CRC
                                        recCRC=RS485_RX_BUFF[RS485_RX_CNT-2]|(((u16)RS485_RX_BUFF[RS485_RX_CNT-1])<<8);//接收到的CRC(低字节在前，高字节在后)
                                        if(calCRC==recCRC)//CRC校验正确
                                        {
                                                /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                                switch(RS485_RX_BUFF[1])//根据不同的功能码进行处理
                                                {
                                                        case 2://读输入开关量
                                                        {
                                                                Modbus_02_Solve();
                                                                break;
                                                        }

                                                        case 1://读输出开关量
                                                        {
                                                                Modbus_01_Solve();
                                                                break;
                                                        }

                                                        case 5://写单个输出开关量
                                                        {
                                                                Modbus_05_Solve();
                                                                break;
                                                        }

                                                        case 15://写多个输出开关量
                                                        {
                                                                Modbus_15_Solve();
                                                                break;
                                                        }

                                                        case 03: //读多个寄存器
                                                        {
                                                                Modbus_03_Solve();
                                                                break;
                                                        }

                                                        case 06: //写单个寄存器
                                                        {
                                                                Modbus_06_Solve();
                                                                break;
                                                        }

                                                        case 16: //写多个寄存器
                                                        {
                                                                Modbus_16_Solve();
                                                                break;
                                                        }

                                                }
                                                //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                        }
                                        else//CRC校验错误
                                        {
                                                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                                                RS485_TX_BUFF[1]=RS485_RX_BUFF[1]|0x80;
                                                RS485_TX_BUFF[2]=0x04; //异常码
                                                RS485_SendData(RS485_TX_BUFF,3);
                                        }        
                                }
                                else//寄存器地址超出范围
                                {
                                        RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                                        RS485_TX_BUFF[1]=RS485_RX_BUFF[1]|0x80;
                                        RS485_TX_BUFF[2]=0x02; //异常码
                                        RS485_SendData(RS485_TX_BUFF,3);
                                }                                                
                        }
                        else//功能码错误
                        {
                                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                                RS485_TX_BUFF[1]=RS485_RX_BUFF[1]|0x80;
                                RS485_TX_BUFF[2]=0x01; //异常码
                                RS485_SendData(RS485_TX_BUFF,3);
                        }
          }

                RS485_FrameFlag=0;//复位帧结束标志
                RS485_RX_CNT=0;//接收计数器清零
                RS485_TX_EN=0;//开启接收模式                
        }                
}
/**
  * @brief          获得8位循环冗余校验值
  * @param[in]     ptr:数组地址
  * @param[in]     len:长度
  * @retval         校验值
  */
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
    unsigned char crc;
    unsigned char i;
    crc = 0;
    while(len--)
    {
        crc ^= *ptr++;
        for(i = 0; i < 8; i++)
        {
            if(crc&0x01)
                crc=(crc>>1)^0x8C;
            else 
                crc >>= 1;
        }
    }
    return crc;
}

bool ModBus_read_bits(int serverAddress)
{
	InitCoilsTab();
	if (IsValid())
	{
		if (modbus_read_bits(m_modBus, serverAddress, 1, m_tab_bit) < 0)
		{
            ROS_INFO("读取线圈数据失败");
			//HandleError(QString(std::string("读取线圈数据失败,地址为:[%1];读取长度:[%2]  ").arg(serverAddress)));
		}
		auto&& c = m_tab_bit[0];
		return m_tab_bit[0];
	}
	return false;
}

void ModBus_write_bits(int serverAddress, bool status)
{
	if (modbus_write_bit(m_modBus, serverAddress, status) < 0)
	{
		HandleError("写入寄存器失败 ");
		//HandleError(QString(std::string("写入寄存器失败,地址为:[%1];写入长度:[%2]  ").arg(serverAddress)));
	}
}
/**
  * @brief         Modbus功能码01处理程序
  * @param[in]      none
  * @retval         none
  */
void Modbus_01_Solve(void)
{
        u16 ByteNum;
        u16 i;
        RegNum= (((u16)RS485_RX_BUFF[4])<<8)|RS485_RX_BUFF[5];//获取寄存器数量
        if((startRegAddr+RegNum)<100)//寄存器地址+数量在范围内
        {
                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                RS485_TX_BUFF[1]=RS485_RX_BUFF[1];
                ByteNum=RegNum/8;//字节数
                if(RegNum%8) ByteNum+=1;//如果位数还有余数，则字节数+1
                RS485_TX_BUFF[2]=ByteNum;//返回要读取的字节数
                for(i=0;i<RegNum;i++)
                {
                        if(i%8==0) RS485_TX_BUFF[3+i/8]=0x00;
                        RS485_TX_BUFF[3+i/8]>>=1;//低位先发送
                        RS485_TX_BUFF[3+i/8]|=((*Modbus_OutputIO[startRegAddr+i])<<7)&0x80;
                        if(i==RegNum-1)//发送到最后一个位了
                        {
                                if(RegNum%8) RS485_TX_BUFF[3+i/8]>>=8-(RegNum%8);//如果最后一个字节还有余数，则剩余MSB填充0
                        }
                }
                calCRC=CRC_Compute(RS485_TX_BUFF,ByteNum+3);
                RS485_TX_BUFF[ByteNum+3]=calCRC&0xFF;
                RS485_TX_BUFF[ByteNum+4]=(calCRC>>8)&0xFF;
                RS485_SendData(RS485_TX_BUFF,ByteNum+5);
        }
        else//寄存器地址+数量超出范围
        {
                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                RS485_TX_BUFF[1]=RS485_RX_BUFF[1]|0x80;
                RS485_TX_BUFF[2]=0x02; //异常码
                RS485_SendData(RS485_TX_BUFF,3);
        }
}
}
/**
  * @brief         Modbus功能码05处理程序
  * @param[in]      none
  * @retval         none
  */
void Modbus_05_Solve(void)
{
        if(startRegAddr<100)//寄存器地址在范围内
        {
                if((RS485_RX_BUFF[4]==0xFF)||(RS485_RX_BUFF[5]==0xFF)) *Modbus_OutputIO[startRegAddr]=0x01;
                else *Modbus_OutputIO[startRegAddr]=0x00;

                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                RS485_TX_BUFF[1]=RS485_RX_BUFF[1];
                RS485_TX_BUFF[2]=RS485_RX_BUFF[2];
                RS485_TX_BUFF[3]=RS485_RX_BUFF[3];
                RS485_TX_BUFF[4]=RS485_RX_BUFF[4];
                RS485_TX_BUFF[5]=RS485_RX_BUFF[5];

                calCRC=CRC_Compute(RS485_TX_BUFF,6);
                RS485_TX_BUFF[6]=calCRC&0xFF;
                RS485_TX_BUFF[7]=(calCRC>>8)&0xFF;
                RS485_SendData(RS485_TX_BUFF,8);
        }
        else//寄存器地址超出范围
        {
                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                RS485_TX_BUFF[1]=RS485_RX_BUFF[1]|0x80;
                RS485_TX_BUFF[2]=0x02; //异常码
                RS485_SendData(RS485_TX_BUFF,3);
        }
}
/**
  * @brief         Modbus功能码06处理程序
  * @param[in]      none
  * @retval         none
  */
void Modbus_06_Solve(void)
{
        *Modbus_HoldReg[startRegAddr]=RS485_RX_BUFF[4];//低字节在前
        *Modbus_HoldReg[startRegAddr]|=((u16)RS485_RX_BUFF[5])<<8;//高字节在后

        RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
        RS485_TX_BUFF[1]=RS485_RX_BUFF[1];
        RS485_TX_BUFF[2]=RS485_RX_BUFF[2];
        RS485_TX_BUFF[3]=RS485_RX_BUFF[3];
        RS485_TX_BUFF[4]=RS485_RX_BUFF[4];
        RS485_TX_BUFF[5]=RS485_RX_BUFF[5];

        calCRC=CRC_Compute(RS485_TX_BUFF,6);
        RS485_TX_BUFF[6]=calCRC&0xFF;
        RS485_TX_BUFF[7]=(calCRC>>8)&0xFF;
        RS485_SendData(RS485_TX_BUFF,8);
}
/**
  * @brief         Modbus功能码16处理程序
  * @param[in]      none
  * @retval         none
  */
void Modbus_16_Solve(void)
{
        u8 i;
        RegNum= (((u16)RS485_RX_BUFF[4])<<8)|RS485_RX_BUFF[5];//获取寄存器数量
        if((startRegAddr+RegNum)<1000)//寄存器地址+数量在范围内
        {
                for(i=0;i<RegNum;i++)
                {
                        *Modbus_HoldReg[startRegAddr+i]=RS485_RX_BUFF[7+i*2]; //低字节在前
                        *Modbus_HoldReg[startRegAddr+i]|=((u16)RS485_RX_BUFF[8+i*2])<<8; //高字节在后
                }
                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                RS485_TX_BUFF[1]=RS485_RX_BUFF[1];
                RS485_TX_BUFF[2]=RS485_RX_BUFF[2];
                RS485_TX_BUFF[3]=RS485_RX_BUFF[3];
                RS485_TX_BUFF[4]=RS485_RX_BUFF[4];
                RS485_TX_BUFF[5]=RS485_RX_BUFF[5];

                calCRC=CRC_Compute(RS485_TX_BUFF,6);
                RS485_TX_BUFF[6]=calCRC&0xFF;
                RS485_TX_BUFF[7]=(calCRC>>8)&0xFF;
                RS485_SendData(RS485_TX_BUFF,8);
        }
        else//寄存器地址+数量超出范围
        {
                RS485_TX_BUFF[0]=RS485_RX_BUFF[0];
                RS485_TX_BUFF[1]=RS485_RX_BUFF[1]|0x80;
                RS485_TX_BUFF[2]=0x02; //异常码
                RS485_SendData(RS485_TX_BUFF,3);
        }
}